{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "050c7614-070d-427b-a02f-e61bd814754e",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "hello there\n"
     ]
    }
   ],
   "source": [
    "print(\"hello there\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c9a44a7d-1d4f-4ae4-acb4-a9daf58cd4f2",
   "metadata": {},
   "outputs": [],
   "source": [
    "import carla\n",
    "import numpy as np\n",
    "import cv2\n",
    "import threading\n",
    "import queue\n",
    "import pygame\n",
    "import time\n",
    "from ultralytics import YOLO\n",
    "\n",
    "# Connect to CARLA\n",
    "client = carla.Client('localhost', 2000)\n",
    "client.set_timeout(10.0)\n",
    "world = client.get_world()\n",
    "\n",
    "# Constants for safety systems\n",
    "ABS_ACTIVATION_PCT = 35\n",
    "FULL_STOP_PCT = 45\n",
    "CENTER_REGION = 0.2\n",
    "MIN_DISTANCE = 20\n",
    "FOCAL_LENGTH = 320\n",
    "VEHICLE_WIDTH = 2.0\n",
    "CROSSWALK_TARGET_SPEED = 20\n",
    "STOP_TARGET_SPEED = 0\n",
    "BASE_BRAKE_FACTOR = 0.3\n",
    "ABS_COOLDOWN = 5\n",
    "STOP_SIGN_HOLD_TIME = 3\n",
    "\n",
    "# Get blueprint library and spawn points\n",
    "blueprint_library = world.get_blueprint_library()\n",
    "spawn_points = world.get_map().get_spawn_points()\n",
    "if not spawn_points:\n",
    "    print(\"No spawn points available. Exiting...\")\n",
    "    exit()\n",
    "\n",
    "# Spawn the vehicle\n",
    "vehicle_bp = blueprint_library.filter('vehicle.dodge.charger')[0]\n",
    "vehicle = world.spawn_actor(vehicle_bp, spawn_points[0])\n",
    "\n",
    "# Attach an RGB camera (CORRECTED CAMERA SETUP)\n",
    "camera_bp = blueprint_library.find('sensor.camera.rgb')\n",
    "camera_bp.set_attribute(\"image_size_x\", \"640\")\n",
    "camera_bp.set_attribute(\"image_size_y\", \"480\")\n",
    "camera_bp.set_attribute(\"fov\", \"90\")\n",
    "camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0))\n",
    "camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle)\n",
    "\n",
    "# Load YOLO models\n",
    "vehicle_model = YOLO(\"vehicle_detector(y11).pt\")\n",
    "road_model = YOLO(\"road_marking_detector.pt\")\n",
    "\n",
    "# Shared variables for safety systems\n",
    "max_vehicle_area = 0.0\n",
    "vehicle_distance = 100.0\n",
    "vehicle_area_rate = 0.0\n",
    "reverse_gear = False\n",
    "prev_area = 0.0\n",
    "prev_time = time.time()\n",
    "crosswalk_detected = False\n",
    "stop_sign_detected = False\n",
    "red_light_detected = False\n",
    "current_speed = 0.0\n",
    "last_abs_activation_time = 0\n",
    "last_stop_time = 0\n",
    "data_lock = threading.Lock()\n",
    "\n",
    "# Frame queue\n",
    "frame_queue = queue.Queue(maxsize=2)\n",
    "\n",
    "def calculate_distance(box_width):\n",
    "    return (VEHICLE_WIDTH * FOCAL_LENGTH) / box_width\n",
    "\n",
    "def get_current_speed():\n",
    "    velocity = vehicle.get_velocity()\n",
    "    return 3.6 * (velocity.x**2 + velocity.y**2 + velocity.z**2)**0.5\n",
    "\n",
    "def image_processing_worker():\n",
    "    global max_vehicle_area, prev_area, prev_time, vehicle_area_rate, vehicle_distance\n",
    "    global crosswalk_detected, stop_sign_detected, red_light_detected\n",
    "    while True:\n",
    "        try:\n",
    "            frame = frame_queue.get(timeout=1)\n",
    "            while not frame_queue.empty():\n",
    "                frame = frame_queue.get()\n",
    "            array = np.frombuffer(frame.raw_data, dtype=np.uint8)\n",
    "            array = array.reshape((frame.height, frame.width, 4))[:, :, :3]\n",
    "            writable_array = array.copy()\n",
    "\n",
    "            height, width = 480, 640\n",
    "            center_left = int(width * (0.5 - CENTER_REGION/2))\n",
    "            center_right = int(width * (0.5 + CENTER_REGION/2))\n",
    "            \n",
    "            cv2.rectangle(writable_array, (center_left, 0), (center_right, height), (0, 165, 255), 2)\n",
    "\n",
    "            # Vehicle detection\n",
    "            vehicle_results = vehicle_model(writable_array, verbose=False)\n",
    "            current_max_area = 0.0\n",
    "            current_distance = 100.0\n",
    "            total_pixels = width * height\n",
    "            \n",
    "            for result in vehicle_results:\n",
    "                for box in result.boxes:\n",
    "                    x1, y1, x2, y2 = map(int, box.xyxy[0])\n",
    "                    class_id = int(box.cls[0])\n",
    "                    class_name = vehicle_model.names[class_id]\n",
    "                    \n",
    "                    if class_name in ['car', 'truck', 'bus', 'motorcycle']:\n",
    "                        box_center_x = (x1 + x2) // 2\n",
    "                        in_center = center_left <= box_center_x <= center_right\n",
    "                        \n",
    "                        # Default color for vehicles outside the center\n",
    "                        color = (0, 255, 0)  # Green\n",
    "                        distance = None\n",
    "                        \n",
    "                        if in_center:\n",
    "                            box_width = x2 - x1\n",
    "                            distance = calculate_distance(box_width)\n",
    "                            \n",
    "                            # Update closest distance and max area\n",
    "                            if distance < current_distance:\n",
    "                                current_distance = distance\n",
    "                                area = (x2 - x1) * (y2 - y1)\n",
    "                                area_percent = (area / total_pixels) * 100\n",
    "                                current_max_area = max(current_max_area, area_percent)\n",
    "                            \n",
    "                            # Set color based on distance for center vehicles\n",
    "                            color = (255, 0, 0) if distance < MIN_DISTANCE else (255, 255, 0)\n",
    "                        \n",
    "                        # Draw bounding box for all vehicles\n",
    "                        cv2.rectangle(writable_array, (x1, y1), (x2, y2), color, 2)\n",
    "                        \n",
    "                        # Add text based on position\n",
    "                        if in_center and distance is not None:\n",
    "                            cv2.putText(writable_array, f\"{distance:.1f}m\", (x1, y1 - 10),\n",
    "                                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)\n",
    "                        else:\n",
    "                            cv2.putText(writable_array, class_name, (x1, y1 - 10),\n",
    "                                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)\n",
    "\n",
    "            current_time = time.time()\n",
    "            time_diff = current_time - prev_time\n",
    "            if time_diff > 0:\n",
    "                area_diff = current_max_area - prev_area\n",
    "                current_rate = (area_diff / time_diff) * 100\n",
    "            else:\n",
    "                current_rate = 0.0\n",
    "\n",
    "            with data_lock:\n",
    "                max_vehicle_area = current_max_area\n",
    "                vehicle_area_rate = current_rate\n",
    "                vehicle_distance = current_distance\n",
    "\n",
    "            prev_area = current_max_area\n",
    "            prev_time = current_time\n",
    "\n",
    "            # Road object detection\n",
    "            road_results = road_model(writable_array, verbose=False)\n",
    "            crosswalk_temp = False\n",
    "            stop_sign_temp = False\n",
    "            red_light_temp = False\n",
    "            \n",
    "            for result in road_results:\n",
    "                for box in result.boxes:\n",
    "                    x1, y1, x2, y2 = map(int, box.xyxy[0])\n",
    "                    confidence = box.conf[0]\n",
    "                    class_id = int(box.cls[0])\n",
    "                    class_name = road_model.names[class_id]\n",
    "                    label = f\"{class_name}: {confidence:.2f}\"\n",
    "                    \n",
    "                    # Draw bounding boxes\n",
    "                    color = (255, 0, 0)\n",
    "                    if class_name == 'Crosswalk':\n",
    "                        color = (0, 255, 255)\n",
    "                        crosswalk_temp = True\n",
    "                    elif class_name == 'Stop_Sign':\n",
    "                        color = (0, 0, 255)\n",
    "                        stop_sign_temp = True\n",
    "                    elif class_name == 'Traffic_Light_Red':\n",
    "                        color = (0, 0, 255)\n",
    "                        red_light_temp = True\n",
    "                    \n",
    "                    cv2.rectangle(writable_array, (x1, y1), (x2, y2), color, 2)\n",
    "                    cv2.putText(writable_array, label, (x1, y2 + 15),\n",
    "                                cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)\n",
    "\n",
    "            with data_lock:\n",
    "                crosswalk_detected = crosswalk_temp\n",
    "                stop_sign_detected = stop_sign_temp\n",
    "                red_light_detected = red_light_temp\n",
    "\n",
    "            cv2.imshow(\"Smart Collision Prevention\", writable_array)\n",
    "            if cv2.waitKey(1) & 0xFF == ord('q'):\n",
    "                break\n",
    "        except queue.Empty:\n",
    "            continue\n",
    "    cv2.destroyAllWindows()\n",
    "\n",
    "worker_thread = threading.Thread(target=image_processing_worker, daemon=True)\n",
    "worker_thread.start()\n",
    "\n",
    "def camera_callback(image):\n",
    "    if not frame_queue.full():\n",
    "        frame_queue.put(image)\n",
    "\n",
    "camera.listen(camera_callback)\n",
    "\n",
    "pygame.init()\n",
    "screen = pygame.display.set_mode((400, 300))\n",
    "pygame.display.set_caption(\"Manual Control (WASD/R Keys)\")\n",
    "\n",
    "# Control parameters\n",
    "throttle = 0.0\n",
    "steering_angle = 0.0\n",
    "brake = 0.0\n",
    "max_steering_angle = 1.0\n",
    "stopped_at_stop = False\n",
    "\n",
    "try:\n",
    "    while True:\n",
    "        current_speed = get_current_speed()\n",
    "        \n",
    "        for event in pygame.event.get():\n",
    "            if event.type == pygame.QUIT:\n",
    "                pygame.quit()\n",
    "                cv2.destroyAllWindows()\n",
    "                camera.destroy()\n",
    "                vehicle.destroy()\n",
    "                exit()\n",
    "            elif event.type == pygame.KEYDOWN:\n",
    "                if event.key == pygame.K_r:\n",
    "                    reverse_gear = not reverse_gear\n",
    "\n",
    "        keys = pygame.key.get_pressed()\n",
    "\n",
    "        if keys[pygame.K_w]:\n",
    "            throttle = 1.0\n",
    "            brake = 0.0\n",
    "        elif keys[pygame.K_s]:\n",
    "            throttle = 0.0\n",
    "            brake = 1.0\n",
    "        else:\n",
    "            throttle = 0.0\n",
    "            brake = 0.0\n",
    "\n",
    "        if keys[pygame.K_a]:\n",
    "            steering_angle = -max_steering_angle\n",
    "        elif keys[pygame.K_d]:\n",
    "            steering_angle = max_steering_angle\n",
    "        else:\n",
    "            steering_angle = 0.0\n",
    "\n",
    "        # Safety systems\n",
    "        auto_brake_active = False\n",
    "        emergency_brake = False\n",
    "        abs_brake = 0.0\n",
    "        traffic_brake = 0.0\n",
    "        \n",
    "        if not reverse_gear:\n",
    "            with data_lock:\n",
    "                current_max = max_vehicle_area\n",
    "                approach_rate = vehicle_area_rate\n",
    "                distance = vehicle_distance\n",
    "                stop_sign = stop_sign_detected\n",
    "                red_light = red_light_detected\n",
    "                crosswalk = crosswalk_detected\n",
    "\n",
    "            # Collision prevention braking\n",
    "            base_brake = 0.0\n",
    "            if current_max >= ABS_ACTIVATION_PCT and distance < MIN_DISTANCE:\n",
    "                base_brake = ((current_max - ABS_ACTIVATION_PCT) / \n",
    "                            (FULL_STOP_PCT - ABS_ACTIVATION_PCT))\n",
    "                distance_factor = 1 - (distance / MIN_DISTANCE)\n",
    "                base_brake = max(base_brake, distance_factor)\n",
    "            \n",
    "            rate_brake = min(approach_rate / 15, 1.0) if approach_rate > 0 else 0.0\n",
    "            abs_brake = min(base_brake + rate_brake, 1.0)\n",
    "            \n",
    "            if distance < MIN_DISTANCE/2 or current_max >= FULL_STOP_PCT:\n",
    "                abs_brake = 1.0\n",
    "                emergency_brake = True\n",
    "\n",
    "            current_time = time.time()\n",
    "            if (current_max >= ABS_ACTIVATION_PCT or emergency_brake) and distance < MIN_DISTANCE:\n",
    "                if current_speed < 5:\n",
    "                    if (current_time - last_abs_activation_time) >= ABS_COOLDOWN:\n",
    "                        auto_brake_active = True\n",
    "                        last_abs_activation_time = current_time\n",
    "                    else:\n",
    "                        auto_brake_active = False\n",
    "                        abs_brake = 0.0\n",
    "                else:\n",
    "                    auto_brake_active = True\n",
    "\n",
    "                if auto_brake_active:\n",
    "                    throttle = 0.0\n",
    "            else:\n",
    "                auto_brake_active = False\n",
    "\n",
    "            # Traffic control braking\n",
    "            if stop_sign or red_light or crosswalk:\n",
    "                target_speed = STOP_TARGET_SPEED if (stop_sign or red_light) else CROSSWALK_TARGET_SPEED\n",
    "                speed_diff = current_speed - target_speed\n",
    "                \n",
    "                if speed_diff > 0:\n",
    "                    traffic_brake = min(BASE_BRAKE_FACTOR * (speed_diff / (target_speed + 1)), 0.8)\n",
    "                    throttle *= 0.3\n",
    "                else:\n",
    "                    traffic_brake = 0.0\n",
    "                    throttle = min(throttle * 1.2, 1.0) if not (stop_sign or red_light) else 0.0\n",
    "\n",
    "                # Full stop logic\n",
    "                if current_speed < 1 and (stop_sign or red_light):\n",
    "                    traffic_brake = 0.8\n",
    "                    throttle = 0.0\n",
    "                    if not stopped_at_stop:\n",
    "                        last_stop_time = current_time\n",
    "                        stopped_at_stop = True\n",
    "                    \n",
    "                    if (current_time - last_stop_time) < STOP_SIGN_HOLD_TIME:\n",
    "                        traffic_brake = 0.5\n",
    "                else:\n",
    "                    stopped_at_stop = False\n",
    "\n",
    "        brake = max(brake, abs_brake, traffic_brake)\n",
    "\n",
    "        # Vehicle control\n",
    "        control = carla.VehicleControl(\n",
    "            throttle=throttle,\n",
    "            steer=steering_angle,\n",
    "            brake=brake,\n",
    "            reverse=reverse_gear\n",
    "        )\n",
    "        vehicle.apply_control(control)\n",
    "\n",
    "        # Display\n",
    "        screen.fill((0, 0, 0))\n",
    "        font = pygame.font.Font(None, 36)\n",
    "        \n",
    "        speed_text = font.render(f\"Speed: {current_speed:.1f} km/h\", True, (255, 255, 255))\n",
    "        screen.blit(speed_text, (10, 10))\n",
    "        \n",
    "        gear_color = (255, 0, 0) if reverse_gear else (0, 255, 0)\n",
    "        gear_text = font.render(\"REVERSE\" if reverse_gear else \"DRIVE\", True, gear_color)\n",
    "        screen.blit(gear_text, (10, 50))\n",
    "        \n",
    "        status_y = 90\n",
    "        if brake > 0:\n",
    "            brake_text = font.render(f\"Braking: {brake*100:.0f}%\", True, (0, 0, 255))\n",
    "            screen.blit(brake_text, (10, status_y))\n",
    "            status_y += 40\n",
    "            \n",
    "            if abs_brake > 0:\n",
    "                status_text = font.render(\"Collision Prevention\", True, (255, 0, 0))\n",
    "                screen.blit(status_text, (10, status_y))\n",
    "                status_y += 40\n",
    "                \n",
    "            if traffic_brake > 0:\n",
    "                status_color = (0, 255, 255)\n",
    "                status_text = \"\"\n",
    "                if crosswalk_detected:\n",
    "                    status_text = \"Crosswalk\"\n",
    "                if stop_sign_detected:\n",
    "                    status_text = \"Stop Sign\"\n",
    "                if red_light_detected:\n",
    "                    status_text = \"Red Light\"\n",
    "                if status_text:\n",
    "                    status_text += \" Speed Control\"\n",
    "                    status_render = font.render(status_text, True, status_color)\n",
    "                    screen.blit(status_render, (10, status_y))\n",
    "                    status_y += 40\n",
    "                \n",
    "        if reverse_gear:\n",
    "            status_text = font.render(\"ABS Disabled\", True, (255, 165, 0))\n",
    "            screen.blit(status_text, (10, status_y))\n",
    "\n",
    "        pygame.display.flip()\n",
    "        world.tick()\n",
    "\n",
    "except KeyboardInterrupt:\n",
    "    print(\"Simulation stopped manually\")\n",
    "\n",
    "finally:\n",
    "    camera.destroy()\n",
    "    vehicle.destroy()\n",
    "    cv2.destroyAllWindows()\n",
    "    pygame.quit()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e8eb7281-347c-4303-a6bc-52b67af3daf6",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.13.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
